#pragma once

#ifndef PI
#define PI 3.14159265359
#endif

#include <list>
using namespace std;

#ifdef _WINDOWS
#include "Win32DemoApplication.h"
#define PlatformDemoApplication Win32DemoApplication
#else
#include "GlutDemoApplication.h"
#define PlatformDemoApplication GlutDemoApplication
#endif

#include "LinearMath/btAlignedObjectArray.h"
#include "btBulletDynamicsCommon.h"
#include "DrawLine.h"
#include "Vector3.h"

#define mFieldSystemSize 1000

void PrintVector3(const btVector3& v);

class Magnet {

public:
	// magnet system
	static int numMagnet;
	static btDynamicsWorld* m_dynamicsWorld;
	static btAlignedObjectArray<btCollisionShape*>	m_collisionShapes;

public:
	static list<Magnet*> MagnetList;
	static const float k;		// u0 / 4pi
	static const float inverseU0;

public:
	static void AddMagnet(Magnet* mg);
	static void SetDynamicsWorld(btDynamicsWorld* dw, btAlignedObjectArray<btCollisionShape*> cs);
	static void Simulation();

	static btVector3 CalcMField(Magnet* mg, const btVector3& pos);

	static void DeleteAllMagnet() {
		for (auto it = MagnetList.begin(); it != MagnetList.end(); it++) {
			delete (*it);
		}
		numMagnet = 0;
	}

	static void DrawMagnetsLine() {
		glDisable (GL_DEPTH_TEST);
		for (auto it =Magnet::MagnetList.begin(); it != MagnetList.end(); it++) {
			(*it)->DrawMagnetMField();
		//	for (int i=0; i<(*it)->numCells; i++) {
		//		(*it)->cells[i].DrawCellMMoment();
		//	}
		}
		glEnable (GL_DEPTH_TEST);
	}

private:

	//RigidBody Info
	btRigidBody* body;

	btScalar mass;
	btScalar density;	// g/cm-3
						// Fe : 7.86 g/cm-3
	bool useDensity;
	btVector3 scale;

	btTransform transform;
	btVector3 position;
	btQuaternion orientation;
	
	btScalar volume;	// m-3 = 10^6 cm-3

	//Magnet Info
	bool isFerroMagnet;
	btScalar permiability;
	btVector3 magnetization;

	bool driver;
	
	//Cell info
	class Cell {
	private:
		btVector3 worldPosition;
		btVector3 localPosition;

		btVector3 mm;

	public:
		Cell() { mm = btVector3(0, 0, 0); };
		void setLocalPosition(const btVector3& p) { localPosition = p; };
		void setMagneticMoment(const btVector3& m) { 
			mm = m; 
#ifdef _DEBUG
			//printf("Cells mm : %f %f %f\n", mm.getX(), mm.getY(), mm.getZ());
#endif
		};
		void updateWorldPosition(const btTransform& t) {
			worldPosition = t * localPosition;
#ifdef _DEBUG
			//printf("Cells pos : %f %f %f\n",worldPosition.getX(),worldPosition.getY(),worldPosition.getZ());
#endif
		};

		const btVector3& getPosition() { return worldPosition; };
		const btVector3& getMoment() { return mm; };

		void DrawCellMMoment() {
			//btVector3 m_sample = Magnet::CalcMField(NULL, worldPosition) * 100;
			btVector3 m_sample = btVector3(0, 10000, 0);
			DrawArrow(worldPosition, worldPosition + m_sample);
		//	PrintVector3(worldPosition);printf("\n");
		}
	};

	int numCells;
	btScalar invNumCells;
	Cell* cells;

	btScalar calcScaleInduce;

public:

	Magnet(int ncell = 1) : 
	  mass(1.), body(NULL), scale(btVector3(1., 1., 1.)), 
	  position(0.,0.,0.),	orientation(0.,0.,0.), 
	  cells(NULL), numCells(1), driver(false), useDensity(false)
	{
		AddMagnet(this);
		
		_CreateCells(ncell);
	}

	~Magnet() {
		if (cells)
			delete [] cells;
	}

	void SetPosition(const btVector3& pos) { position = pos; };
	Vector3 GetPosition(){ return position;};
	void SetMass(const btScalar& m) { mass = m; if (useDensity) density = mass / volume; };
	void SetDensity(const btScalar& d) { if (useDensity) {density = d; mass = volume * density;} };
	void SetScale(const btVector3& sc) { scale = sc; _SetVolume(); _UpdatecalcScaleInduce(); if (useDensity) mass = volume * density; };
	bool UseDensity(bool u = true) { return (useDensity = u); };
	void SetOrientation(const btQuaternion& q) { orientation = q; };
	void SetFerroMagnet(bool setFerro = true) { isFerroMagnet = setFerro; };
	void SetMagnetization(const btVector3& mm) { magnetization = mm; };
	Vector3 GetMagnetization(){return magnetization;};
	void SetPermiability(btScalar p) { permiability = p; _UpdatecalcScaleInduce(); };
	int GetNumCells(){return numCells;};
	Cell GetCell(int i){return cells[i];};

	void RegisterToWorld();

	bool Equal(Magnet* mg) {
		return (mg == this);
	};

	const btVector3 DrawMagnetMField() {
		btTransform tf = body->getWorldTransform();
		btVector3 pos = tf.getOrigin();
		btVector3 mf = CalcMField(this, pos);
		DrawArrow(pos, pos+mf/100.);

		return mf;
	};

	static const Vector3 GetMagnetForce(const Vector3& pk, const Vector3& mk);
	static const Vector3 ComputeMagnetForce(const Vector3& pos, const Vector3& other_pos, const Vector3& moment, const Vector3& other_moment);
	static const Vector3 ComputeMagnetField(const Vector3& pos, const Vector3& magnet_pos, const Vector3& moment);

private:

	void _SetCellLocalPosition();
	void _UpdateCellPosition();
	void _UpdateCellMagneticMoment();
	void _UpdatecalcScaleInduce() { 
		calcScaleInduce = 3 * permiability * inverseU0 * volume / ( ( 1 + permiability) * numCells ); 
	};

	void _ComputeForceAndTorque(Magnet *mgl);

	void _CreateCells() {
		_CreateCells(numCells);
	}
	void _CreateCells(int ncell) {
		int _n = ncell;

		numCells = 1;
		while (_n > 1) {
			_n /= 2;
			numCells *= 2;
		}

		if (cells) delete [] cells;

		cells = new Cell[numCells];
		invNumCells = 1. / numCells;
	};

	btVector3 _ComputeMField(const btVector3& pos);
	void _ComputeInducedMagnetization();

	void _SetVolume() {
		volume = scale.x() * scale.y() * scale.z();
	};
	


public:
	static Vector3 _ComputeMFieldSystem(Vector3& pos);


};


void PrintVector3(const btVector3& v);